

#include "oslib_can.h"
#include "can_func.h"
#include "aiming_cmd.h"
#include "claw_signal.h"
#ifdef OSLIB_UART_MODULE_ENABLED
#include "oslib_uart.h"
#endif
#include "shooter.h"
#include "handle.h"
#include "dji_board_can.h"
#include "base_chassis.h"

#if defined(OSLIB_CAN_MODULE_ENABLED)

/*-Declaration--------------------------------------------*/

/* 在这里添加CAN回调函数的定义或外部声明 */
#ifdef OSLIB_UART_MODULE_ENABLED
// static void CanCallback_EchoID(CAN_ConnMessage *msg)
// {
//     uprintf("CAN: Callback[%x]\r\n", msg->id);
// }
#endif

extern osSemaphoreId_t AimInitSema;
/**
 * @brief homing 回调函数
 * 
 */
static void CanCallback_HomingOver(CAN_ConnMessage *msg)
{
    // uprintf("CAN: Homing over[%x]\r\n", msg->id);
    int16_t boardId = msg->payload.i16[0];
    int16_t motorId = msg->payload.i16[1];
    uprintf("homing over, boardId is %d, motorId is %d\r\n", boardId, motorId);
    if (boardId == 1)
    {
        if (motorId == 1)
        {
            // osSemaphoreRelease(AimInitSema);
            shooter_yaw_homing_ready(shooter1);
        }
        else if (motorId == 2)
        {
            shooter_pitch_homing_ready(shooter1);
        }
    } // TODO: 爪子大疆反馈函数
    else if (boardId == 2)
    {
        Claw_DjiResetOK(boardId, motorId);
    }
}

/**
 * @brief 位置环回调函数
 * 
 */
static void CanCallback_PosArrival(CAN_ConnMessage *msg)
{
    // uprintf("CAN: Homing over[%x]\r\n", msg->id);
    int16_t boardId = msg->payload.i16[0];
    int16_t motorId = msg->payload.i16[1];
    uprintf("POS arrival, boardId is %d, motorId is %d\r\n", boardId, motorId);
    if (boardId == 1)
    {
        if (motorId == 1)
        {
            // osSemaphoreRelease(AimInitSema);
            shooter_yaw_ready(shooter1);
        }
        else if (motorId == 2)
        {
            shooter_pitch_ready(shooter1);
        }
    } // TODO: 爪子大疆反馈函数
    else if (boardId == 2)
    {
        Claw_DjiPosOK(boardId, motorId);
    }
}

static void CanCallback_ReadInfo(CAN_ConnMessage *msg)
{
    int motorId = (msg->payload.ui8[0] >> 6) & 0xff;
    uint16_t velSign = (msg->payload.ui8[0] & 0x20) << 10;
    int16_t vel = (msg->payload.ui8[0] & 0x1f) << 8;
    vel |= msg->payload.ui8[1];
    uprintf("v=%d s=%uhd\r\n", vel, velSign);
    if (velSign != 0)
    {
        vel |= velSign | (1 << 14) | (1 << 13);
    }
    int cur = msg->payload.i16[1];
    float pos = msg->payload.fl[1];
    uprintf("motorId=%d\r\n vel=%d\r\n pos=%f\r\n cur=%d\r\n", motorId, vel, pos, cur);
}

/* 在这里添加任务对应消息队列的外部声明 */
#if USE_OSLIB_CAN_EXAMPLE
extern osMessageQueueId_t ExampleCanTaskQueue;
#endif
extern osMessageQueueId_t ShooterVescQueueHandle;
extern osMessageQueueId_t EncoderExQueueHandle;
extern osMessageQueueId_t CtrlBtnQueueHandle;

/*-IDList--------------------------------------------------*/

/* 
 * 以下IDList数组每一项格式为 {<ID>, <ID类型>, <消息队列地址>, <回调函数>}
 * 注意1: <消息队列地址> 和 <回调函数> 两域有且仅有一个有值, 另一个写为NULL
 * 注意2: VescList的ID类型只能为CAN_IDTYPE_EXT, 
 * 注意3: VescList目前只支持任务方式处理, 即 <回调函数> 为NULL, <消息队列地址> 有值
 * 注意4: 两个List中的所有ID不能有冲突, 包括VESC扩展出去的那些ID. 
 *      (比如VescList中有ID[0x58], NormList中有ID[0x158], 会产生冲突.)
 * 注意5: 两个数组中的ID数量有限制, 每个CAN大约要求 2*VescSize+NormSize < 28
 */

#if defined(CAN1_ENABLED)

#if USE_OSLIB_CAN1_VESC
/* 在这里添加对CAN1的VESC的ID和对应消息队列的指针 */
CAN_IDRecord_t CAN1_VescList[] = {
#if USE_OSLIB_CAN_EXAMPLE
    {0x58, CAN_IDTYPE_EXT, &ExampleCanTaskQueue, NULL},
#endif
    {0x59, CAN_IDTYPE_EXT, &ShooterVescQueueHandle, NULL},
};

uint32_t CAN1_VescSize = sizeof(CAN1_VescList) / sizeof(CAN_IDRecord_t);
#endif

/* 在这里添加对CAN1的其他ID和对应消息队列的指针或回调函数 */
CAN_IDRecord_t CAN1_NormList[] = {
    {641, CAN_IDTYPE_STD, NULL, CAN_Callback_DJI_ReadInfo},
    {642, CAN_IDTYPE_STD, NULL, CAN_Callback_DJI_ReadAllPosInfo},
    {204, CAN_IDTYPE_STD, NULL, CAN_Callback_Location_ReadPos_X},
    {205, CAN_IDTYPE_STD, NULL, CAN_Callback_Location_ReadPos_Y},
    {206, CAN_IDTYPE_STD, NULL, CAN_Callback_Location_ReadPos_Yaw},
#if USE_OSLIB_CAN_EXAMPLE
    {0x120, CAN_IDTYPE_STD, &ExampleCanTaskQueue, NULL},

#endif
};

uint32_t CAN1_NormSize = sizeof(CAN1_NormList) / sizeof(CAN_IDRecord_t);
#endif

#if defined(CAN2_ENABLED)
#if USE_OSLIB_CAN2_VESC
/* 在这里添加对CAN2的VESC的ID和对应消息队列的指针 */
CAN_IDRecord_t CAN2_VescList[] = {
#if USE_OSLIB_CAN_EXAMPLE
    {0x15, CAN_IDTYPE_EXT, &ExampleCanTaskQueue, NULL},
#endif
};

uint32_t CAN2_VescSize = sizeof(CAN2_VescList) / sizeof(CAN_IDRecord_t);
#endif

/* 在这里添加对CAN2的其他ID和对应消息队列的指针或回调函数 */
CAN_IDRecord_t CAN2_NormList[] = {
    {324, CAN_IDTYPE_STD, NULL, CAN_Callback_Handle_Rocker},
    // {325, CAN_IDTYPE_STD, NULL, CAN_Callback_Handle_Button},
    {0x285, CAN_IDTYPE_STD, NULL, CanCallback_HomingOver},
    {0x286, CAN_IDTYPE_STD, NULL, CanCallback_PosArrival},
    {0xEC, CAN_IDTYPE_STD, &EncoderExQueueHandle, NULL},
    {0x146, CAN_IDTYPE_STD, &CtrlBtnQueueHandle, NULL},
    {0x281, CAN_IDTYPE_STD, NULL, CanCallback_ReadInfo},
    // {0xED, CAN_IDTYPE_STD, &EncoderExQueueHandle, NULL},
#if USE_OSLIB_CAN_EXAMPLE
    {0x100, CAN_IDTYPE_STD, &ExampleCanTaskQueue, NULL},
#endif
};

uint32_t CAN2_NormSize = sizeof(CAN2_NormList) / sizeof(CAN_IDRecord_t);
#endif

#endif
